#ifndef I_LIB_VISIONAD_I_CAMMODEL_H
#define I_LIB_VISIONAD_I_CAMMODEL_H

#include <iostream>
#include "../core/i_blas.h"

namespace idl {
    template<typename DataType>
    struct ADCameraExtrinsic {
        DataType R[9]; /*Rotation matrix*/
        DataType t[3]; /*translation vector*/
        
        ADCameraExtrinsic() {
            i_eye_3x3<DataType>(R);
            i_zero3(t);
        }
        
        explicit ADCameraExtrinsic(const ADCameraExtrinsic<DataType>& pose) {
            i_copy9(pose.R, R);
            i_copy3(pose.t, t);
        }
        
        ADCameraExtrinsic& operator=(const ADCameraExtrinsic<DataType>& pose) {
            i_copy9(pose.R, this->R);
            i_copy3(pose.t, this->t);
            return(*this);
        }
        /*friend std::ostream& operator << (std::ostream& out, const ADCameraExtrinsic& rhs) {
            out << "R: ";
            for (int i = 0; i < 9; ++i) out << rhs.R[i] << " "; out << std::endl;
            out << "t: ";
            for (int i = 0; i < 3; ++i) out << rhs.t[i] << " "; out << std::endl;
            out << "cop: ";
            DataType cop[3];
            i_cop_from_R_t(rhs.R, rhs.t, cop);
            for (int i = 0; i < 3; ++i) out << cop[i] << " "; out << std::endl;
            return out;
        }*/
        
        DataType cop_z() const {
            return -(R[2] * t[0] + R[5] * t[1] + R[8] * t[2]);
        }

        void reset() {
            i_eye_3x3<DataType>(R);
            i_zero3(t);
        }
    };

    template<typename DataType>
    struct ADCameraIntrinsic {
        DataType  K[9]; /*K matrix*/
        DataType Ki[9]; /*inverse of K*/
        
        ADCameraIntrinsic() {
            i_eye_3x3<DataType>(K);
            i_eye_3x3<DataType>(Ki);
        }
        
        ADCameraIntrinsic(DataType fx, DataType fy, DataType px, DataType py) {
            /*K  matrix - the calibration matrix, note K[1] = 0*/
            K[0] = fx; 
            K[1] = 0; 
            K[2] = px;
            K[3] = 0; 
            K[4] = fy; 
            K[5] = py;
            K[6] = 0; 
            K[7] = 0; 
            K[8] = (DataType)1.0;

            /*Ki matrix - the inverse of the calibration matrix K*/
            DataType fx_rec = i_rec(K[0]);
            DataType fy_rec = i_rec(K[4]);

            Ki[0] = fx_rec;
            Ki[1] = 0;
            Ki[2] = -(K[2] * fx_rec);

            Ki[3] = 0;
            Ki[4] = fy_rec;
            Ki[5] = -(K[5] * fy_rec);

            Ki[6] = 0;
            Ki[7] = 0;
            Ki[8] = (DataType)1.0;
        }

        explicit ADCameraIntrinsic(const DataType calibMatrix[9]) {
            /*K  matrix - the calibration matrix, assume calibMatrix[1] = 0*/
            i_copy9(calibMatrix, K);

            /*Ki matrix - the inverse of calibration matrix K*/
            DataType fx_rec = i_rec(K[0]);
            DataType fy_rec = i_rec(K[4]);

            Ki[0] = fx_rec;
            Ki[1] = 0;
            Ki[2] = -(K[2] * fx_rec);

            Ki[3] = 0;
            Ki[4] = fy_rec;
            Ki[5] = -(K[5] * fy_rec);

            Ki[6] = 0;
            Ki[7] = 0;
            Ki[8] = (DataType)1.0;
        }

        explicit ADCameraIntrinsic(const ADCameraIntrinsic<DataType>& rhs) {
            i_copy9(rhs.K, K);
            i_copy9(rhs.Ki, Ki);
        }
        
        ADCameraIntrinsic& operator=(const ADCameraIntrinsic<DataType>& rhs) {
            i_copy9(rhs.K, K);
            i_copy9(rhs.Ki, Ki);
            return(*this);
        }
    };
}/*namespace idl*/

#endif
